﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Mapenect.GesturePatterns;

namespace Mapenect.Data
{
    class MyVector
    {
        private float _x;
        public float X
        {
            get { return _x; }
            set { _x = value; }
        }
        private float _y;
        public float Y
        {
            get { return _y; }
            set { _y = value; }
        }
        private float _z;
        public float Z
        {
            get { return _z; }
            set { _z = value; }
        }

        public MyVector(float x, float y, float z)
        {
            X = x;
            Y = y;
            Z = z;
        }

        public MyVector(MyJoint joint1, MyJoint joint2)
        {
            X = joint2.X - joint1.X;
            Y = joint2.Y - joint1.Y;
            Z = joint2.Z - joint1.Z;
        }

        //Compares this and another Vector within a given Threshold
        public Boolean CompareTo(MyVector vector,float threshold)
        {
            MyVector thisVector = this.Normalize();
            MyVector otherVector = vector.Normalize();

            if(compareValues(thisVector.X, otherVector.X, threshold) &&
                compareValues(thisVector.Y, otherVector.Y, threshold) &&
                compareValues(thisVector.Z, otherVector.Z, threshold))
            {
                return true;
            }
            else
            {
                return false;
            }

            
        }

        //returns Norm of Vector (Betrag)
        public float GetNorm()
        {
            return (float)Math.Sqrt(Math.Pow(X, 2) + Math.Pow(Y, 2) + Math.Pow(Z, 2));
        }

        //Normalizes this Vector and returns it as a new one
        public MyVector Normalize()
        {
            float distance = GetNorm();
            return new MyVector(X/distance, Y/distance, Z/distance);
        }

        //sums up Vectors
        public void AddVector(MyVector vector)
        {
            X += vector.X;
            Y += vector.Y;
            Z += vector.Z;
        }

        //Compares to float valus under a given threshold
        private Boolean compareValues(float value1, float value2, float threshold)
        {
            if((value2 > value1 - threshold) && (value2 < value1 + threshold))
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        // Returns Angle between two Vectors
        public float GetAngle(MyVector vector)
        {
            float scalar = X * vector.X + Y * vector.Y + Z * vector.Z;
            float norm = GetNorm() * vector.GetNorm();

            return (float)Math.Acos(scalar / norm);
        }

        public string GetString()
        {
            return "" + X + "," + Y + ", " + Z;
        }
    }
}
